/*
--------------------------------------------------------------------------------
				Ak-Lib. 
			 Project       	: Ak-Lib
			 Group         	: Ak-Lib R&D Team
			 Author        	: Ocean.Wang
			 Create Date 	: 05-01-2010
--------------------------------------------------------------------------------
Code Name 	:  A.K 	~v~
Description	:  Yet Another Cross-Platform Lib.
Cautions    	:
--------------------------------------------------------------------------------
Change History:
Version		: 0.1
Building 		: 05/01/10
10-05.01 first draft, build, debug and test.
--------------------------------------------------------------------------------
*/

#include "ak_msgq.h"

static 	int 	_gTraceLvl_MsgQ = TRACELVL_WARNING;

#define 	_MSGSLOT_INIT(pSlotMsg, pWake) \
	do{\
		SYS_INIT_CS(&((pSlotMsg)->lock));\
		BLIST_INIT(&((pSlotMsg)->queue));\
		(pSlotMsg)->wake = (pWake);\
	}while(0)

#define 	_MSGSLOT_NODE_GET(pSlotMsg, pMsgNode) \
	do{\
		SYS_ENTER_CS(&((pSlotMsg)->lock));\
		pMsgNode = (_t_sysMsgNode *)BLST_GET(&((pSlotMsg)->queue));\
		SYS_LEAVE_CS(&((pSlotMsg)->lock));\
	}while(0)

#define 	_MSGSLOT_NODE_ADD(pSlotMsg, pMsgNode) \
	do{\
		SYS_ENTER_CS(&((pSlotMsg)->lock));\
		BLST_ADD2T(&((pSlotMsg)->queue), &((pMsgNode)->node));\
		SYS_LEAVE_CS(&((pSlotMsg)->lock));\
		SYS_SEM_B_GIVE((pSlotMsg)->wake);\
	}while(0)

#define 	_MSGNODE_ENBUF(pSlotMsg, pMsgNode, pBuf, msgBuf, msgLen) \
	do{\
		/* Copy & Enque */\
		pBuf = (uInt8 *)((pMsgNode)+1);	(pMsgNode)->len = msgLen;\
		SYS_MEMCPY(pBuf+sizeof(uInt32),  msgBuf, msgLen);\
		_MSGSLOT_NODE_ADD(pSlotMsg, pMsgNode);\
	}while(0)

#define 	_MSGNODE_DEBUF(pSlotFree, pMsgNode, pBuf, lBuf, msgBuf, msgLen) \
	do{\
		/* Copy & Fee */\
		pBuf = (uInt8 *)((pMsgNode)+1); lBuf= (pMsgNode)->len;\
		if(lBuf <= msgLen){\
			SYS_MEMCPY(msgBuf, pBuf+sizeof(uInt32),  lBuf);\
		}else{\
			lBuf = 0;\
		}\
		/* Free It */\
		_MSGSLOT_NODE_ADD(pSlotFree, pMsgNode);\
	}while(0)


_t_sysMsgQ	*	SMsgQ_Create(uInt32 msgMaxNum, uInt32 msgMaxLen)
{
	_t_sysMsgQ 		* pMsgQ;
	_t_sysMsgSlot	* pSlotMsg;
	_t_sysMsgSlot	* pSlotFree;
	_t_sysMsgNode	* pMsgNode;
	uInt32 			i, msgLen;

	
	/* Init the MsgQ Struct */
	pMsgQ = (_t_sysMsgQ*)SYS_MALLOC(sizeof(_t_sysMsgQ));

	if(NULL != pMsgQ){
		/* Alloc the Msg */
		msgMaxNum = (msgMaxNum < 16) ? 16 : msgMaxNum;
		msgMaxLen = (msgMaxLen < 16) ? 16 : msgMaxLen;

		msgLen = sizeof(_t_sysMsgNode) + msgMaxLen;/* Msg Size */; 
		pMsgNode = (_t_sysMsgNode*)SYS_MALLOC(msgLen * msgMaxNum);

		if(pMsgNode){
			/* Init the Queue */
			pMsgQ->magic 	  = SMSGQ_MAGIC;	
			pMsgQ->msgMaxNum  = msgMaxNum;
			pMsgQ->msgMaxLen  = msgMaxLen;
			pMsgQ->msgRx = 0;
			pMsgQ->msgTx = 0;

			/* Init the Wake */
			SYS_SEM_B_INIT(&(pMsgQ->wakeFree));
			SYS_SEM_B_INIT(&(pMsgQ->wakeMsg));
			/* Init the  Free Slot */
			pSlotFree = &(pMsgQ->free);
			_MSGSLOT_INIT(pSlotFree, &(pMsgQ->wakeFree));
			while(msgMaxNum--){
				BLST_ADD2T(&(pSlotFree->queue), &(pMsgNode->node));
				pMsgNode = (_t_sysMsgNode *)((uInt8*)pMsgNode + msgLen);
			}
			
			/* Init the Pri-Slot*/
			for(i=0, pSlotMsg = pMsgQ->slots; i<_MSGQ_PRI_MAX; i++, pSlotMsg++){
				_MSGSLOT_INIT(pSlotMsg, &(pMsgQ->wakeMsg));
			}
			return pMsgQ;
			
		}
		else{
			SYS_FREE(pMsgQ);
		}
	}
	return NULL;
}

sInt32			SMsgQ_Peek(_t_sysMsgQ * pMsgQ)
{
	_t_sysMsgSlot	* 	pSlotMsg;
	int 				i, count;
	count = 0;
	if(	pMsgQ && (pMsgQ->magic == SMSGQ_MAGIC)){
		for(i=0, pSlotMsg = pMsgQ->slots; i<_MSGQ_PRI_MAX; i++, pSlotMsg++){
			count += pSlotMsg->queue.count;
		}
	}
	return count;
}

sInt32	SMsgQ_Send(_t_sysMsgQ * pMsgQ, void * msgBuf, uInt32 msgLen, sInt32 msgPri, sInt32 timeOut)
{
	_t_sysMsgSlot	* pSlotMsg;
	_t_sysMsgSlot	* pSlotFree;
	_t_sysMsgNode	* pMsgNode;
	uInt8  			* pBuf;
	sInt32 			ret;
	
	ret = _E_FAIL;
	/* Verify the MsgQ & Msg */
	if(	pMsgQ && (pMsgQ->magic == SMSGQ_MAGIC) && 
		msgBuf && (msgLen <= pMsgQ->msgMaxLen) &&
		(msgPri >= _MSGQ_PRI_LO) && 
		(msgPri <= _MSGQ_PRI_HI)){
		/* Free Slot */
		pSlotFree = &(pMsgQ->free);
		_MSGSLOT_NODE_GET(pSlotFree, pMsgNode);

		if(!pMsgNode){
			/* Waiting & Retry */
			SYS_SEM_B_TAKE((pSlotFree->wake), timeOut);
			_MSGSLOT_NODE_GET(pSlotFree, pMsgNode);
		}
		/* Check the Node */
		if(pMsgNode){
			pSlotMsg = &(pMsgQ->slots[msgPri]);
			_MSGNODE_ENBUF(pSlotMsg, pMsgNode, pBuf, msgBuf, msgLen);

			/* Stat */
			pMsgQ->msgTx++;
			ret = msgLen;
		}else{
			SYS_TRACE(_gTraceLvl_MsgQ, TRACELVL_ERROR, "MsgQSend QFull Error!\n");
		}
	}

	return ret;
}

sInt32	SMsgQ_Recv(_t_sysMsgQ * pMsgQ, void * msgBuf, uInt32 msgLen, sInt32 timeOut)
{
	_t_sysMsgSlot	* pSlotMsg;
	_t_sysMsgSlot	* pSlotFree;
	_t_sysMsgNode	* pMsgNode;
	uInt8  			* pBuf;
	uInt32			i, lBuf;
	
	/* Verify the MsgQ & Msg */
	lBuf = -1;
	pMsgNode= NULL;
	if(	pMsgQ && (pMsgQ->magic == SMSGQ_MAGIC) && 
		msgBuf && (msgLen > 0)){
		/* Lock-Free Peek */
		for(i=0, pSlotMsg = pMsgQ->slots; i<_MSGQ_PRI_MAX; i++, pSlotMsg++){
			if(pSlotMsg->queue.count > 0){
				_MSGSLOT_NODE_GET(pSlotMsg, pMsgNode);
				if(pMsgNode){
					pSlotFree = &(pMsgQ->free);
					_MSGNODE_DEBUF(pSlotFree, pMsgNode, pBuf, lBuf, msgBuf, msgLen);
					break;
				}
			}
		}

		if(!pMsgNode){
			/* Waiting & Retry */
			SYS_SEM_B_TAKE((pSlotMsg->wake), timeOut);
			for(i=0, pSlotMsg = pMsgQ->slots; i<_MSGQ_PRI_MAX; i++, pSlotMsg++){
				if(pSlotMsg->queue.count > 0){
					_MSGSLOT_NODE_GET(pSlotMsg, pMsgNode);
					if(pMsgNode){
						pSlotFree = &(pMsgQ->free);
						_MSGNODE_DEBUF(pSlotFree, pMsgNode, pBuf, lBuf, msgBuf, msgLen);
						break;
					}
				}
			}
		}
	}

	return lBuf;
}



